//
// Created by xuan on 2024/4/21.
//

#include "UPRE_Bsp_UART_R1_F405.h"
#include "UPRE_ROBOT_TASK.h"
#include "UPRE_DIJKSTRA.h"
#include "stdio.h"

ManualMode manual = {0};

static uint8_t receive_buf = 0;

bool once_togo = false;

bool manualMode_flag = false;

static union Switch_UINT8_T2Float_ {
    uint8_t P[4];
    float value;
} Switch_UINT8_T2Float;


static uint8_t Rec;

#define RXBUFFERSIZE  256     //最大接收字节数
uint8_t RxBuffer[RXBUFFERSIZE]; //接收数据
uint8_t Uart4_Rx_Cnt = 0; //接收缓冲计数


uint8_t get_uartReceive(void) {
    return receive_buf = Rec;
}

static uint8_t ManualMode_rec[9];
static uint8_t PathTo_rec[4];

void ManualMode_Receive(void) {
    if (ManualMode_rec[0] == 0xc0) {
        Switch_UINT8_T2Float.P[0] = ManualMode_rec[1];
        Switch_UINT8_T2Float.P[1] = ManualMode_rec[2];
        Switch_UINT8_T2Float.P[2] = ManualMode_rec[3];
        Switch_UINT8_T2Float.P[3] = ManualMode_rec[4];
        manual.ManualMode_velocity_dir = Switch_UINT8_T2Float.value;
        Switch_UINT8_T2Float.P[0] = ManualMode_rec[5];
        Switch_UINT8_T2Float.P[1] = ManualMode_rec[6];
        Switch_UINT8_T2Float.P[2] = ManualMode_rec[7];
        Switch_UINT8_T2Float.P[3] = ManualMode_rec[8];
        manual.ManualMode_velocity_size = Switch_UINT8_T2Float.value;
        printf("%f %f\r\n", manual.ManualMode_velocity_dir, manual.ManualMode_velocity_size);
    }
    for (int i = 0; i < 9; ++i) {
        printf("%x\r\n", ManualMode_rec[i]);
    }
    ManualMode_Init();
}

void PathFeedback_to_Master(uint8_t path_knots_num) {
    uint8_t PathFeedback_tx[3] = {0};
    PathFeedback_tx[0] = 0x88;
    PathFeedback_tx[1] = path_knots_num;
    PathFeedback_tx[2] = 0x11;
    HAL_UART_Transmit(&huart4, PathFeedback_tx, sizeof PathFeedback_tx, 0xffff);
}

void MasterSlave_communication_Init(void) {
    HAL_UART_Receive_IT(&huart4, PathTo_rec, 4);
}

static bool getBallColor(const uint8_t *color) {
    uint8_t bit_array[16] = {0};
    bool check_flag = false;
    for (int i = 0; i < 16; ++i) {
        bit_array[i] = ((color[(i / 8) + 1] >> (i % 8)) & 0x01);
    }
    for (int i = 0; i < 3; ++i) {
        if ((bit_array[i + 8] & 0x01) != !(bit_array[i + 12] & 0x01)) {
            break;
        } else {
            check_flag = true;
        }
    }
    if (check_flag) {
        for (int i = 0; i < 12; ++i) {
            ball[i / 6][i % 6] = bit_array[i];
            printf(" %d ", ball[i / 6][i % 6]);
        }
        printf("\r\n");
    } else {
    }
    return DIJKSTRA_CheckBall();
}

void MasterSlave_communication_Receive(void) {
    if (PathTo_rec[0] == 0x11) {
        ////TODO:上层执行完毕，进行反馈，继续路径。
        if (once_togo) {
            if ((PathTo_rec[1] & 0x01)) {
                g_Auto_Num++;
                once_togo = false; ////TODO：在需要等待上层运行的路径函数位置将“once_togo”置为true
            }
        } else {
            NULL;
        }
        printf("%d\r\n", g_Auto_Num);
        once_togo = true;
    }

    if (PathTo_rec[0] == 0x22) {
        if (getBallColor(RxBuffer)) {
            printf("Ball_OK!!!\r\n");
            uint8_t Feedback_ballState[3] = {0};
            Feedback_ballState[0] = 0x33;
            Feedback_ballState[1] = 0x01;
            Feedback_ballState[2] = 0x03;
            HAL_UART_Transmit(&huart4, Feedback_ballState, sizeof Feedback_ballState, 0xffff);
        } else {
            printf("Ball_ERROR!!!\r\n");
            uint8_t Feedback_ballState[3] = {0};
            Feedback_ballState[0] = 0x33;
            Feedback_ballState[1] = 0x00;
            Feedback_ballState[2] = 0x03;
            HAL_UART_Transmit(&huart4, Feedback_ballState, sizeof Feedback_ballState, 0xffff);
        }
    }
    for (int i = 0; i < 4; ++i) {
        printf("%x\r\n", PathTo_rec[i]);
    }

    MasterSlave_communication_Init();
}

void Communicate_Init(void) {
    HAL_UART_Receive_IT(&huart4, &Rec, 1);
}

void Communicate_Receive(void) {
    if (Uart4_Rx_Cnt >= 255) //溢出判断
    {
        Uart4_Rx_Cnt = 0;
        memset(RxBuffer, 0x00, sizeof(RxBuffer));
        HAL_UART_Transmit(&huart5, (uint8_t *) "data_error\r\n", 12, 0xFFFF);
    } else {
        RxBuffer[Uart4_Rx_Cnt++] = Rec; //接收数据转存

        if ((RxBuffer[Uart4_Rx_Cnt - 1] == 0x0A) && (RxBuffer[Uart4_Rx_Cnt - 2] == 0x0D)) //判断结束位
        {
            HAL_UART_Transmit(&huart5, (uint8_t *) &RxBuffer, Uart4_Rx_Cnt, 0xFFFF); //将收到的信息发送出去
            while (HAL_UART_GetState(&huart5) == HAL_UART_STATE_BUSY_TX); //检测UART发送结束
            Uart4_Rx_Cnt = 0;
            memset(RxBuffer, 0x00, sizeof(RxBuffer)); //清空数组
        }
        if ((/*RxBuffer[0] == 0x11 && */RxBuffer[Uart4_Rx_Cnt - 1] == 0x01)) {
            if (once_togo) {
                if ((RxBuffer[1] & 0x01)) {
                    g_Auto_Num++;
                    once_togo = false; ////TODO：在需要等待上层运行的路径函数位置将“once_togo”置为true
                }
            } else {
                NULL;
            }
            printf("%d\r\n", g_Auto_Num);
            once_togo = true;
            while (HAL_UART_GetState(&huart4) == HAL_UART_STATE_BUSY_TX); //检测UART发送结束
            Uart4_Rx_Cnt = 0;
            memset(RxBuffer, 0x00, sizeof(RxBuffer)); //清空数组
        }
        if (((/*RxBuffer[0] == 0x22 && */RxBuffer[Uart4_Rx_Cnt - 1] == 0x02))) {
            if (getBallColor(RxBuffer)) {
                printf("Ball_OK!!!\r\n");
                uint8_t Feedback_ballState[3] = {0};
                Feedback_ballState[0] = 0x33;
                Feedback_ballState[1] = 0x01;
                Feedback_ballState[2] = 0x03;

                HAL_UART_Transmit(&huart4, Feedback_ballState, sizeof Feedback_ballState, 0xffff);
                while (HAL_UART_GetState(&huart4) == HAL_UART_STATE_BUSY_TX); //检测UART发送结束
                Uart4_Rx_Cnt = 0;
                memset(RxBuffer, 0x00, sizeof(RxBuffer)); //清空数组
            } else {
                printf("Ball_ERROR!!!\r\n");
                uint8_t Feedback_ballState[3] = {0};
                Feedback_ballState[0] = 0x33;
                Feedback_ballState[1] = 0x00;
                Feedback_ballState[2] = 0x03;

                HAL_UART_Transmit(&huart4, Feedback_ballState, sizeof Feedback_ballState, 0xffff);

                while (HAL_UART_GetState(&huart4) == HAL_UART_STATE_BUSY_TX); //检测UART发送结束
                Uart4_Rx_Cnt = 0;
                memset(RxBuffer, 0x00, sizeof(RxBuffer)); //清空数组
            }
        }
        if ((/*RxBuffer[0] == 0x11 && */RxBuffer[Uart4_Rx_Cnt - 1] == 0x04)) {
            manualMode_flag = RxBuffer[1] & 0x01;
        }
        if (((/*RxBuffer[0] == 0xc0 && */RxBuffer[Uart4_Rx_Cnt - 1] == 0x0c))) {
            Switch_UINT8_T2Float.P[0] = RxBuffer[1];
            Switch_UINT8_T2Float.P[1] = RxBuffer[2];
            Switch_UINT8_T2Float.P[2] = RxBuffer[3];
            Switch_UINT8_T2Float.P[3] = RxBuffer[4];
            manual.ManualMode_velocity_dir = Switch_UINT8_T2Float.value;
            Switch_UINT8_T2Float.P[0] = RxBuffer[5];
            Switch_UINT8_T2Float.P[1] = RxBuffer[6];
            Switch_UINT8_T2Float.P[2] = RxBuffer[7];
            Switch_UINT8_T2Float.P[3] = RxBuffer[8];
            manual.ManualMode_velocity_size = Switch_UINT8_T2Float.value;
            printf("%f %f\r\n", manual.ManualMode_velocity_dir, manual.ManualMode_velocity_size);
            while (HAL_UART_GetState(&huart4) == HAL_UART_STATE_BUSY_TX); //检测UART发送结束
            Uart4_Rx_Cnt = 0;
            memset(RxBuffer, 0x00, sizeof(RxBuffer)); //清空数组
        }
    }

    Communicate_Init();
}
